// #include <bsp/peripheral/new_bmi088.h>
// #include "bsp/Algorithm/alg_qekf.h"

// extern "C" {
// #include "BSXlite/bsxlite_interface.h"
// }
// #define ERR() OsError("bmi088 err")

// using namespace units::literals;

// namespace bsp {
// SpiDevice::SpiDevice(SPI_HandleTypeDef* hspi) : _hspi{hspi} {}

// SpiDevice* SpiDevice::init(SPI_InitTypeDef config) {
//     _hspi->Init = config;
//     auto err = HAL_SPI_Init(_hspi);
//     if (err != HAL_OK) {
//         ERR();
//     }

//     return this;
// }

// SpiDevice* SpiDevice::receiveDataDMA(uint8_t* buffer, uint16_t len) {
//     if ((_hspi == nullptr) || (buffer == nullptr)) {
//         ERR();
//     }

//     auto err = HAL_SPI_Receive_DMA(_hspi, buffer, len);
//     if (err != HAL_OK) {
//         ERR();
//     }
//     return this;
// }

// SpiDevice* SpiDevice::receiveData(uint8_t* buffer, uint16_t len, uint32_t timeout) {
//     if ((_hspi == nullptr) || (buffer == nullptr)) {
//         ERR();
//     }

//     auto err = HAL_SPI_Receive(_hspi, buffer, len, timeout);
//     if (err != HAL_OK) {
//         ERR();
//     }
//     return this;
// }

// SpiDevice* SpiDevice::sendDataDMA(uint8_t* buffer, uint16_t len) {
//     if ((_hspi == nullptr) || (buffer == nullptr)) {
//         ERR();
//     }

//     auto err = HAL_SPI_Transmit_DMA(_hspi, buffer, len);
//     if (err != HAL_OK) {
//         ERR();
//     }
//     return this;
// }

// SpiDevice* SpiDevice::sendData(uint8_t* buffer, uint16_t len, uint32_t timeout) {
//     if ((_hspi == nullptr) || (buffer == nullptr)) {
//         ERR();
//     }

//     auto err = HAL_SPI_Transmit(_hspi, buffer, len, timeout);
//     if (err != HAL_OK) {
//         ERR();
//     }
//     return this;
// }

// SpiDevice* SpiDevice::swapOneByteData(uint8_t tx_data, uint8_t* rx_data_ptr) {
//     auto ret = HAL_SPI_TransmitReceive(_hspi, &tx_data, rx_data_ptr, 1, 100);
//     if (ret != HAL_OK) {
//         ERR();
//     }
//     return this;
// }

// SpiDevice* SpiDevice::readMultiReg(uint8_t command, uint8_t* rx_data_ptr, uint8_t length) {
//     while (length--) {
//         swapOneByteData(command, rx_data_ptr++);
//     }
//     return this;
// }

// SpiDevice* SpiDevice::swapDataDMA(uint8_t* tx_data_ptr, uint8_t* rx_data_ptr, uint16_t len) {
//     if ((_hspi == nullptr) || (tx_data_ptr == nullptr) || (rx_data_ptr == nullptr)) {
//         ERR();
//     }

//     auto err = HAL_SPI_TransmitReceive_DMA(_hspi, tx_data_ptr, rx_data_ptr, len);
//     if (err != HAL_OK) {
//         ERR();
//     }
//     return this;
// }

// SpiDevice* SpiDevice::swapDataDMA(uint8_t* tx_data_ptr, uint8_t* rx_data_ptr, uint16_t len, uint32_t timeout) {
//     if ((_hspi == nullptr) || (tx_data_ptr == nullptr) || (rx_data_ptr == nullptr)) {
//         ERR();
//     }

//     auto err = HAL_SPI_TransmitReceive(_hspi, tx_data_ptr, rx_data_ptr, len, timeout);
//     if (err != HAL_OK) {
//         ERR();
//     }
//     return this;
// }

// }  // namespace bsp

// namespace bsp {
// GpioDriver::GpioDriver(GPIO_TypeDef* gpio, uint16_t pin, GPIO_PinState state) : _gpio{gpio}, _pin{pin}, _state{state} {}

// GpioDriver* GpioDriver::reset() {
//     HAL_GPIO_WritePin(_gpio, _pin, GPIO_PIN_RESET);
//     read();
//     return this;
// }

// GpioDriver* GpioDriver::set() {
//     HAL_GPIO_WritePin(_gpio, _pin, GPIO_PIN_SET);
//     read();
//     return this;
// }

// GPIO_PinState GpioDriver::read() {
//     _state = HAL_GPIO_ReadPin(_gpio, _pin);
//     return _state;
// }

// GpioDriver* GpioDriver::toggle() {
//     HAL_GPIO_TogglePin(_gpio, _pin);
//     read();
//     return this;
// }
// }  // namespace bsp

// extern SPI_HandleTypeDef hspi2;

// namespace {
// bsp::GpioDriver chip_select_accel{GPIOB, GPIO_PIN_12};
// bsp::GpioDriver chip_select_gyro{GPIOB, GPIO_PIN_11};
// bsp::SpiDevice bmi088_spi{&hspi2};
// os::QuaternionExtendedKalmanFilter qekf_n{10, 0.01F, 10000000, 1, 0.0085F};
// Bmi088 gimbal_imu{&bmi088_spi, &chip_select_accel, &chip_select_gyro};
// bsp::bmi088::InsDataType gimbal_ins{};

// units::time::second_t begin;
// units::time::second_t end;
// units::angle::degree_t yaw_last = 0.0_deg;
// units::angle::degree_t pitch_last = 0.0_deg;
// units::angle::degree_t roll_last = 0.0_deg;
// }  // namespace

// vector_3d_t accel_data = {0, 0, 0};
// vector_3d_t gyro_data = {0, 0, 0};
// bsxlite_instance_t ins_instance;
// bsxlite_out_t bsx_out;

// namespace bsp {
// namespace bmi088 {
// Quaternion_DataTypeDef quaternion_data;
// void Start() {
//     using namespace units::literals;
//     using namespace units::time;
//     using namespace units::angle;

//     gimbal_imu.init();
//     // bsxlite_init(&ins_instance);
//     QEKF_Init(&(quaternion_data), 10, 0.01f, 10000000, 1, 0.0085f);

// }

// uint32_t tim;

// float normalize_angle(float angle) {
//     // 将角度化到 -180 到 +180 之间
//     while (angle > 180.0f) {
//         angle -= 360.0f;
//     }
//     while (angle < -180.0f) {
//         angle += 360.0f;
//     }
//     return angle;
// }

// units::angle::degree_t angle_difference(float current, float last) {
//     // 计算角度差，并确保它在 -180 到 180 之间
//     float diff = current - last;
//     return units::angle::degree_t(normalize_angle(diff));
// }

// int time_round_count = 0; // 爆int32_t计数
// void Update() {
//     begin = end;
//     end = os::GetDWTTime() - 2146_s * time_round_count;
//     if (end.to<uint32_t>() > 2146) ++time_round_count;
//     gimbal_imu.update();

//     accel_data = {gimbal_imu.accel_measurements.x(), gimbal_imu.accel_measurements.y(),
//                   gimbal_imu.accel_measurements.z()};
//     gyro_data = {gimbal_imu.gyro_measurements.pitch(), gimbal_imu.gyro_measurements.roll(),
//                  gimbal_imu.gyro_measurements.yaw()};

//     // 更新四元数
//     bsxlite_do_step(&ins_instance, end.convert<units::time::microsecond>().value(), &accel_data, &gyro_data, &bsx_out);

//     // 获取最终数据
//     gimbal_ins.pitch = units::angle::radian_t(bsx_out.orientation.pitch);
//     gimbal_ins.roll = units::angle::radian_t(bsx_out.orientation.roll);
//     gimbal_ins.yaw = units::angle::radian_t(bsx_out.orientation.yaw);  // 0~360

//     if (gimbal_ins.yaw - yaw_last > 180.0_deg) {
//         gimbal_ins.yaw_round--;
//     } else if (gimbal_ins.yaw - yaw_last < -180.0_deg) {
//         gimbal_ins.yaw_round++;
//     }
//     gimbal_ins.yaw_total_angle = 360.0_deg * gimbal_ins.yaw_round + gimbal_ins.yaw;

//     gimbal_ins.pitch_speed =
//         angle_difference(units::unit_cast<float>(gimbal_ins.pitch), units::unit_cast<float>(pitch_last)) /
//         (end - begin);
//     gimbal_ins.roll_speed =
//         angle_difference(units::unit_cast<float>(gimbal_ins.roll), units::unit_cast<float>(roll_last)) / (end - begin);
//     gimbal_ins.yaw_speed =
//         angle_difference(units::unit_cast<float>(gimbal_ins.yaw), units::unit_cast<float>(yaw_last)) / (end - begin);
//     pitch_last = gimbal_ins.pitch;
//     roll_last = gimbal_ins.roll;
//     yaw_last = gimbal_ins.yaw;

// }

// InsDataType const& GetImuDataRef() { return gimbal_ins; }
// }  // namespace bmi088
// }  // namespace bsp